#include "positionloop.hpp"
#include <assert.h>
#include <iostream>
#include <math.h>

#define IF_DEBUG true

PositionLoop::PositionLoop(double _ts):
    ts(_ts)
{
    assert(ts>0.0);
}

void PositionLoop::initPidController_parallel(int initValue, double _kp, double _ki, double _kd)
{
    lastPos = initValue;
    kp = _kp;
}

int PositionLoop::getPidOut_int(int refer, int current) const
{
    return kp*(refer-current);
}

double PositionLoop::getPidOut_double(int refer, int current) const
{
    return kp*(refer-current);
}

PositionLoop::~PositionLoop()
{
}

void PositionLoop::initVelEst(double _diffN, double initVel)
{
    diffN = _diffN;
    velEst = initVel;
}

double PositionLoop::velEstimate(int Pos)
{
//    velEst = (Pos - lastPos) / ts;
    velEst = (diffN*(Pos - lastPos)+velEst) / (diffN*ts+1);
    lastPos = Pos;
    return velEst;
}

void PositionLoop::initRcFilter(int time, int initValue)
{
    if(time >0){
        rcTime = time;
    }
    filterOut = initValue;
}

int PositionLoop::getRcFilterOut(int inPuls)
{
    filterOut = ((rcTime * filterOut + (inPuls<<5)) / (rcTime+1));
    if(filterOut/2==1){
        filterOut++;
    }
//    if(IF_DEBUG)
//    {
//        std::cout << "PositionLoop::getRcFilterOut====" << inPuls <<" "<< rcTime <<" "<< (filterOut>>5)  << std::endl;//<< " "<<(int)tmp
//    }
    return filterOut>>5;
}

int PositionLoop::loopOut(int refer, int current)
{
    int refer_filter = getRcFilterOut(refer);
    return getPidOut_double(refer_filter, current)+ velEstimate(refer_filter);
//    return getPidOut_int(refer_filter, current);
}
